#include <stdio.h>
#include <string.h>

#include "scorbot.h"
#include "queue.h"

//#define DEBUG

extern QUEUE_HANDLE scorbotWrite_handle;	//From X to Write thread
extern QUEUE_HANDLE scorbotRead_handle;	//From write thread to read thread
extern QUEUE_HANDLE scorbotOutput_handle;	//from read thread to X

int HOMED = 1;
int SETUP = 1;
int ARRIVED = 0;
poDesc_t CURRENT_POS;

int setupQueueElem(QUEUE_ELEM *elem, char *cmd, char *term,
		   int ignore, int expected, int timeout) {
    memset(elem, 0, sizeof(*elem));
    strcpy(elem->rw.cmd, cmd);
    strcpy(elem->rw.terminator, term);
    elem->rw.ignoreNumLines = ignore;
    elem->rw.expectedLines  = expected;
    elem->rw.expectedTime   = timeout;

    return 0;
}

int scorbot_Home() {
    // Init command
    QUEUE_ELEM elem;

    printf("HOMING! :\n");

    setupQueueElem(&elem, "HOME", "Homing complete(robot).", 2, 1, 120);

    // Push onto write queue
    queue_push(scorbotWrite_handle, &elem);
    // Pop off of output queue
    queue_pop(scorbotOutput_handle, &elem);

	HOMED = 1;
	
    return 0;
}

//shouldn't need this as long as we have a globally defined POS for the current position
int scorbot_Listpv() {
    // LISTPV command
    QUEUE_ELEM elem;

	if(HOMED != 1) {
		printf("NOT HOMED!\n");
		return -1;
	}

    printf("LISTPV! :\n");

    setupQueueElem(&elem, "LISTPV OURS", "", 2, 2, 1);

//    printf("Pushing \"%s\" onto write queue\n", elem.rw.cmd);
    // Push onto write queue
    queue_push(scorbotWrite_handle, &elem);

    // Pop off of output queue
    queue_pop(scorbotOutput_handle, &elem);

//    printf("Read \"%s\" back from robot\n", elem.data);
	
    return 0;
}

int open_gripper() {
    QUEUE_ELEM elem;

    printf("OPENING! :\n");

    setupQueueElem(&elem, "OPEN", "", 1, 1, 3);

    // Push onto write queue
    queue_push(scorbotWrite_handle, &elem);

    // Pop off of output queue
    queue_pop(scorbotOutput_handle, &elem);

	return 0;
}
int close_gripper() {
    QUEUE_ELEM elem;

    printf("CLOSING! :\n");

    setupQueueElem(&elem, "CLOSE", "", 1, 1, 3);

    // Push onto write queue
    queue_push(scorbotWrite_handle, &elem);

    // Pop off of output queue
    queue_pop(scorbotOutput_handle, &elem);

	return 0;
}

int setSpeed(int s) {
	QUEUE_ELEM elem;
	char str[16] = { 0 };

	printf("SETTING SPEED TO %d\n", s);

	sprintf(str, "SPEED %d", s);	
	setupQueueElem(&elem, str, "Done.", 1, 1, 1);
	queue_push(scorbotWrite_handle, &elem);
	queue_pop(scorbotOutput_handle, &elem);
}

int Setup()	{
    //create the position to use/ edit in runtime for new positions
    QUEUE_ELEM elem;

    printf("SETUP! :\n");

	if(HOMED != 1 && scorbot_Home() != 0) {
		return -1;
    }

	printf("scorbot_Home() returned\n");

	//We need to create the transformation matrix here


	//we need to calibrate after being homed


	//We need to get images from the camera after being homed


	SETUP = 1;

    return 0;
}

int scorbot_MoveC(char* name, int timeout)	{
    QUEUE_ELEM elem;

	if(HOMED != 1) {
		printf("NOT HOMED!\n");
		return -1;
	}

    setupQueueElem(&elem, name, "Done.", 1, 1, 1);

//    printf("Pushing \"%s\" onto write queue\n", elem.rw.cmd);
    queue_push(scorbotWrite_handle, &elem);

    queue_pop(scorbotOutput_handle, &elem);
	sleep(timeout);
	return 0;
//    printf("Moved.\n");
}

int scorbot_MoveAbs(poDesc_t *pos, int wait) {
    //This takes in a poDesc_t called toMove, it has:
    //double x, y, z;
    //double theta, rho, phi;
    //This will send a move command to the scorbot based on that.
/*
  DEFP OURS <-- already defined
  TEACH OURS\rX\rY\xZ\rP\rR
  MOVE OURS
*/
    QUEUE_ELEM elem;

	if(HOMED != 1) {
		printf("NOT HOMED!\n");
		return -1;
	}

    setupQueueElem(&elem, "TEACH OURS", "", 1, 0, 1);
//    printf("Pushing \"%s\" onto write queue\n", elem.rw.cmd);
    queue_push(scorbotWrite_handle, &elem);
    queue_pop(scorbotOutput_handle, &elem); // no output

    setupQueueElem(&elem, "", "", 0, 1, 1);
    sprintf(elem.rw.cmd, "%d", (int) pos->x);
//    printf("Pushing \"%s\" (%f) onto write queue\n", elem.rw.cmd, pos->x);
    queue_push(scorbotWrite_handle, &elem);
    queue_pop(scorbotOutput_handle, &elem);
//    printf("Read \"%s\" back from robot\n", elem.data);
	
    setupQueueElem(&elem, "", "", 0, 1, 1);	
    sprintf(elem.rw.cmd, "%d", (int) pos->y);
//    printf("Pushing \"%s\" onto write queue\n", elem.rw.cmd);
    queue_push(scorbotWrite_handle, &elem);
    queue_pop(scorbotOutput_handle, &elem);
//    printf("Read \"%s\" back from robot\n", elem.data);
	
    setupQueueElem(&elem, "", "", 0, 1, 1);	
    sprintf(elem.rw.cmd, "%d", (int) pos->z);
//    printf("Pushing \"%s\" onto write queue\n", elem.rw.cmd);
    queue_push(scorbotWrite_handle, &elem);
    queue_pop(scorbotOutput_handle, &elem);
//    printf("Read \"%s\" back from robot\n", elem.data);
	
    setupQueueElem(&elem, "", "", 0, 1, 1);	
    sprintf(elem.rw.cmd, "%d", (int) pos->phi);
//    printf("Pushing \"%s\" onto write queue\n", elem.rw.cmd);
    queue_push(scorbotWrite_handle, &elem);
    queue_pop(scorbotOutput_handle, &elem);
//    printf("Read \"%s\" back from robot\n", elem.data);
	
    setupQueueElem(&elem, "", "Done.", 0, 2, 1);	
    sprintf(elem.rw.cmd, "%d", (int) pos->rho);
//    printf("Pushing \"%s\" onto write queue\n", elem.rw.cmd);
    queue_push(scorbotWrite_handle, &elem);
    queue_pop(scorbotOutput_handle, &elem);
//    printf("Read \"%s\" back from robot\n", elem.data);

    setupQueueElem(&elem, "MOVE OURS", "Done.", 2, 1, 1);
//    printf("Pushing \"%s\" onto write queue\n", elem.rw.cmd);
    queue_push(scorbotWrite_handle, &elem);
    queue_pop(scorbotOutput_handle, &elem);
//    printf("Read \"%s\" back from robot\n", elem.data);

	ARRIVED = 1;

	sleep(wait);
    return 0;
}

int driver_init() {
    char c;

#if defined(DEBUG)
    printf("Creating serial_fd to enable connection\n");
#endif

    if (0 > commDriver_Init())
		return -1;

    //SETUP
	if(Setup() != 0) { 
		return -1; 
	}

	printf("Done setting up:\n");

#if defined(DEBUG)
    printf("Serial port/USB working correctly, Robot is homed, commence searching for box:\n");
#endif

    return 0;
}
